#ifndef MLTOOL_TRIANGULAR_SOLVER_H
#define MLTOOL_TRIANGULAR_SOLVER_H

#include<cassert>
#include"layout.h"
#include"tags.h"
#include"triangular_matrix.h"

namespace math{
	//Lx = b
	template<class Mat, class Vec>
	inline int inplace_solve(const Mat &L, Vec &b, lower_tag, vector_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == b.size());
#endif
		typedef typename Vec::size_type size_type;
		typedef typename Mat::value_type value_type;
		typedef typename Vec::value_type T;

		for(size_type i = 0; i < b.size(); ++i)
		{
			if(L(i,i) != value_type())
			{
				T tmp = 1.0 / L(i,i);
				for(size_type j = 0; j < i; ++j)
					b[i] -= L(i,j) * b[j];
				b[i] *= tmp;
			}
			else
				return -1;
		}
		return 0;
	}

	// LX = B
	template<class Mat1, class Mat2>
	inline int inplace_solve(const Mat1& L, Mat2 &B, lower_tag, matrix_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == B.size1());
#endif
		typedef typename Mat2::size_type size_type;
		typedef typename Mat1::value_type value_type;
		typedef typename Mat2::value_type T;

		for(size_type i = 0; i < L.size1(); ++i)
		{
			if(L(i,i) != value_type())
			{
				T tmp = 1.0 / L(i,i);
				for(size_type j = 0; j < i; ++j)
					for(size_type k = 0; k < B.size2(); ++k)
						B(i,k) -= L(i,j)*B(j,k);
				for(size_type k = 0; k < B.size2(); ++k)
					B(i,k) *= tmp;
			}
			else
				return -1;
		}
		return 0;
	}

	//ULx = b
	template<class Mat, class Vec>
	inline int inplace_solve(const Mat& L, Vec &b, unit_lower_tag, vector_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == b.size());
#endif

		int sz = b.size();
		for(int i = 1; i < sz ; ++i)
			for(int j = 0; j < i; ++j)
				b[i] -= L(i,j)*b[j];
		return 0;
	}

	//ULX = B
	template<class Mat1, class Mat2>
	inline int inplace_solve(const Mat1& L, Mat2 &B, unit_lower_tag, matrix_tag)
	{
#ifdef CHECK_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size1() == B.size1());
#endif
		typedef typename Mat2::size_type size_type;

		for(size_type i = 1; i < L.size1(); ++i)
			for(size_type j = 0; j < i; ++j)
				for(size_type k = 0; k < B.size2(); ++k)
					B(i,k) -= L(i,j)*B(j,k);
		return 0;
	}

	//solve Ux = b
	template<class Mat, class Vec>
	inline int inplace_solve(const Mat& U, Vec &b, upper_tag, vector_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == b.size());
#endif

		typedef typename Mat::value_type value_type;
		typedef typename Vec::value_type T;

		int sz = b.size();
		for(int i = sz - 1; i >=0; --i)
		{
			if(U(i,i) != value_type())
			{
				T tmp = 1.0 / U(i,i);
				for(int j = i + 1; j < sz; ++j)
					b[i] -= U(i,j) * b[j];
				b[i] *= tmp;
			}
			else
				return -1;
		}
		return 0;
	}


	//UUx = b
	template<class Mat, class Vec>
	inline int inplace_solve(const Mat& U, Vec &b, unit_upper_tag, vector_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == b.size());
#endif

		int sz = b.size();
		for(int i = sz - 2; i >=0; --i)
			for(int j = i + 1; j < sz; ++j)
				b[i] -= U(i,j)*b[j];
		return 0;
	}

	//UX = B
	template<class Mat1, class Mat2>
	inline int inplace_solve(const Mat1& U, Mat2 &B, upper_tag, matrix_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == B.size1());
#endif
		typedef typename Mat1::value_type value_type;
		typedef typename Mat2::value_type T;

		int sz = B.size1();
		for(int i = sz - 1; i >=0; --i)
		{
			if(U(i,i) != value_type())
			{
				T tmp = 1.0 / U(i,i);
				for(int j = i + 1; j < sz; ++j)
					for(int k = 0; k < B.size2(); ++k)
						B(i,k) -= U(i,j) * B(j, k);
				for(int k = 0; k < B.size2(); ++k)
					B(i,k) *= tmp;
			}
			else
				return -1;
		}
		return 0;
	}

	//UUX = B
	template<class Mat1, class Mat2>
	inline int inplace_solve(const Mat1& U, Mat2 &B, unit_upper_tag, matrix_tag)
	{
#ifdef CHECH_DIMENSION_MATCH
		assert(L.size1() == L.size2());
		assert(L.size2() == B.size1());
#endif

		int sz = B.size1();
		for(int i = sz - 2; i >=0; --i)
			for(int j = i + 1; j < sz; ++j)
				for(int k = 0; k < B.size2(); ++k)
					B(i,k) -= U(i,j) * B(j, k);
		return 0;
	}

	template<class T1, class T2, class TRI>
	inline int inplace_solve(const T1& A, T2 &B, TRI tri_tag)
	{
		int tmp = inplace_solve(A, B, tri_tag, typename T2::category());
		if(tmp == -1)
			std::cerr << " coefficient matrix is singular! " << std::endl;
		return tmp;
	}

	/*
	Solve Ax = B
	Input :
	Mat ---- the type of the coefficient matrix A. It must be a 
	triangular_matrix<T, TRI>.
	Vec ---- the type of the righthand vector. It meant to be a
	vector<T> or matrix<T>.
	A   ---- the coefficient matrix.
	B   ---- the righthand vector(or matrix). it will be overwritten by
	x.
	Output :
	The function returning 0 means that equation Ax = B has been solved successfully, while
	returning -1 means that A is singular and it failed to solve Ax = B.
	*/
	template<class triMat, class Vec>
	inline int inplace_solve(const triMat &A, Vec &B)
	{
		return inplace_solve(A, B, typename triMat::triangualr_category(), typename Vec::category());
	}

}//namespace
#endif
